About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Target tracking from raw radar detections

Description: Guide to filtering raw detections into tracked object detections

Tutorial Level: INTERMEDIATE

Next Tutorial: Tracking object Cartesian pose

Overview

Most radar sensors designed for applications in autonomous systems output what are known informally as raw detections (or targets). These are the direct result of applying standard peak-finding algorithms like CFAR on returned signals converted to the frequency domain.

The "peaks" of this multidimensional data correspond to detected objects; depending on the algorithm used, the resolutions of the sensor and so on, there may be many such detections - for example, from the K79 imaging radar. In this case, we may not necessarily care about the geometry of an object inferred from the group of targets but rather seek a stable detection of the object over time for applications like collision avoidance. We refer to this as a tracked detection (target), and it involves additional processing either on or offboard the radar in order to aggregate information from the many individual returns.

Background

The basic idea behind implementing a filter which outputs tracked targets from periodic scans of raw targets is to maintain the state of potential objects and update them with new data as it arrives, and publishing these objects only when they have been stably tracked for some time.

Consider the figure below. In the top row, the detections evolve over time as the objects move around; even as the cross one another's paths, assuming the framerate is high enough, it's not terribly difficult to "connect the dots" in range-doppler space to track them. The second row is much more realistic, however - there are many returns from each object, plus background noise, which make it difficult to tell where the objects move and even how many distinct objects are in the scene. The final row shows the solution we seek for the tracking problem.

Theory and Implementation

The tracking filter is essentially a bank of simple Kalman filters, each maintaining the state of a tracked object in spherical coordinates. The flowchart below describes roughly how the filter works.

Essentially, a main thread continually updates the filters in the bank using the process model, while new radar data triggers a callback in which detections (targets) are evaluated against the existing filters in the bank. If a target is consistent with a filter's state, it passes through that filter's validation gate and triggers a Kalman filter update. If a target is not consistent with any filter, it is pushed back as a new filter. If a filter has not been updated within a specified timeout, it is removed from the bank.

The state of each instance of the filter is

latex error! exitcode was 1 (signal 0), transscript follows:

This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex)

kpathsea: Running mktexfmt latex.fmt
mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order):
mktexfmt:   /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf
mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes:
mktexfmt:   /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf
/usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied
I can't find the format file `latex.fmt'!

and the process model is simply

latex error! exitcode was 1 (signal 0), transscript follows:

This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex)

kpathsea: Running mktexfmt latex.fmt
mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order):
mktexfmt:   /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf
mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes:
mktexfmt:   /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf
/usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied
I can't find the format file `latex.fmt'!

Or, in the matrix form

latex error! exitcode was 1 (signal 0), transscript follows:

This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex)

kpathsea: Running mktexfmt latex.fmt
mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order):
mktexfmt:   /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf
mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes:
mktexfmt:   /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf
/usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied
I can't find the format file `latex.fmt'!

we can write

where F is the state transition matrix, L is the process noise matrix, and

latex error! exitcode was 1 (signal 0), transscript follows:

This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex)

kpathsea: Running mktexfmt latex.fmt
mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order):
mktexfmt:   /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf
mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes:
mktexfmt:   /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf
/usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied
I can't find the format file `latex.fmt'!

is the process noise vector. The noise terms

latex error! exitcode was 1 (signal 0), transscript follows:

This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex)

kpathsea: Running mktexfmt latex.fmt
mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order):
mktexfmt:   /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf
mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes:
mktexfmt:   /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf
/usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied
I can't find the format file `latex.fmt'!

are the process noise on speed, azimuth and elevation angles, respectively. These are each assumed to be normally distributed with specified standard deviations.

The measurement model simply measures the state exactly, plus additive noise:

latex error! exitcode was 1 (signal 0), transscript follows:

This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex)

kpathsea: Running mktexfmt latex.fmt
mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order):
mktexfmt:   /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf
mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes:
mktexfmt:   /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf
/usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied
I can't find the format file `latex.fmt'!

Or, in the matrix form

latex error! exitcode was 1 (signal 0), transscript follows:

This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex)

kpathsea: Running mktexfmt latex.fmt
mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order):
mktexfmt:   /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf
mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes:
mktexfmt:   /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf
/usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied
I can't find the format file `latex.fmt'!

we can write

where H is the measurement model matrix and

latex error! exitcode was 1 (signal 0), transscript follows:

This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex)

kpathsea: Running mktexfmt latex.fmt
mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order):
mktexfmt:   /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf
mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes:
mktexfmt:   /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf
/usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied
I can't find the format file `latex.fmt'!

is the measurement noise vector. The noise terms

latex error! exitcode was 1 (signal 0), transscript follows:

This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex)

kpathsea: Running mktexfmt latex.fmt
mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order):
mktexfmt:   /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf
mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes:
mktexfmt:   /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf
/usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied
I can't find the format file `latex.fmt'!

are the measurement noise on range, speed, azimuth and elevation angles, respectively. These are each assumed to be normally distributed with specified standard deviations.

Usage

As detailed on the ainstein_radar_filters documentation, the tracking filter node has the following structure:

Subscribed Topics

~radar_in (ainstein_radar_msgs/RadarTargetArray)

• Radar detections (targets) to be used for outputting tracked targets.

Published Topics

~tracked (ainstein_radar_msgs/RadarTargetArray)

• Tracked targets output after updating Kalman Filters.

~boxes (jsk_recognition_msgs/BoundingBoxArray)

• Bounding boxes corresponding to each tracked target, formed from all raw detections associated with that tracked target after the current update.

Parameters

~filter_update_rate (int, default: 20)

• Rate at which the filter is run, in Hz.

~filter_min_time (float, default: 0.5)

• Minimum time a target must be tracked before being published, in seconds (DYNAMICALLY-RECONFIGURABLE).

~filter_timeout (float, default: 0.5)

• Maximum time a tracked target can not be updated before being dropped, in seconds (DYNAMICALLY-RECONFIGURABLE).

~filter_val_gate_thresh (string map, default: 90.0% confidence)

• Validation gate confidence threshold map - see here for options (DYNAMICALLY-RECONFIGURABLE).

In most cases, it should be sufficient to tune the four filter parameters above based on the application - see below for some guidance on tuning. Always rosrun rqt_reconfigure rqt_reconfigure to tune these parameters dynamically, then save your settings and load them using functionality from dynamic_reconfigure.

• filter_update_rate: This is the fixed rate at which the tracking is run, allowing to fill in the gaps between radar scans and provide tracked object information continuously. When no radar data is available, the simple kinematic model is integrated using the current state estimate for each object, so the object could drift considerably before being dropped from the filter.

• filter_min_time: This is the time, in seconds, an object must be tracked before it's published in the tracked targets array. For fast-moving objects (eg tracking cars) this time should be set lower, however lowering the minimum time can lead to noise appearing as tracked targets.

• filter_timeout: This is the time, in seconds, an object can be tracked purely from the kinematic model before being dropped due to lack of radar measurement updates. This should be set according to the radar's actual output rate, with a longer timeout for slower output rates. Of course, a longer timeout also means that targets may be spuriously tracked even when they're not visible to the sensor.

• filter_val_gate_thresh: This is the threshold which, roughly, controls how much measurement error to allow when updating a tracked target from raw detections. It's specified as a confidence level percentage on the radar data, with the idea being that a higher confidence level (up to 99.5%) means much stricter use of data in updating tracked targets than a low one (down to 25.0%). The implication is that high confidence allows multiple targets in the vicinity of one another to be tracked independently, while a low confidence might merge those targets. It's easier to keep a stable track on an object with a low confidence, but it also means that the estimate is easily corrupted by data which is far from the true position. Consider again the problem in the image below. In the final scan, three objects are still clearly visible, but they begin to overlap; with a low confidence, these objects may merge into a single tracked object. With a high confidence, we may preserve the three object tracks but also start tracking an object from the noise down in the bottom left, for example.

Kalman Filter Tuning

The default parameters for the underlying Kalman Filter which is run per tracked object were tuned for people tracking using the K79 imaging radar. For other radars and applications, it may be necessary to re-tune these parameters for better performance. The parameters and their descriptions are listed below.

Parameters

Initial Uncertainty Parameters

These parameters describe the initial uncertainty when the filter is initialized. Since the filter is initialized from an actual radar measurement, this should be based on the radar resolution.

~kf_init_range_stdev (float, default: 1.0)

• Initial range standard deviation, in meters.

~kf_init_speed_stdev (float, default: 2.0)

• Initial speed standard deviation, in meters per second.

~kf_init_azim_stdev (float, default: 1.0)

• Initial azimuth angle standard deviation, in degrees.

~kf_init_elev_stdev (float, default: 1.0)

• Initial elevation angle standard deviation, in degrees.

Process Model Noise Parameters

These parameters describe the additive process model uncertainties. These are functions of the accuracy of the process model; since we use random walks to model the dynamics of the angles, their uncertainty is set high.

~kf_q_speed_stdev (float, default: 5.0)

• Speed process noise standard deviation, in meters per second.

~kf_q_azim_stdev (float, default: 10.0)

• Azimuth angle process noise standard deviation, in degrees.

~kf_q_elev_stdev (float, default: 10.0)

• Elevation angle process noise standard deviation, in degrees.

Measurement Model Noise Parameters

These parameters describe the additive measurement model uncertainties. These are again loosely based on the radar data resolution/accuracy, however they were tuned with considerably higher uncertainty than that of the radar data in order to promote smooth tracking specifically for the people tracking application.

~kf_r_range_stdev (float, default: 1.0)

• Range measurement noise standard deviation, in meters.

~kf_r_speed_stdev (float, default: 5.0)

• Speed measurement noise standard deviation, in meters per second.

~kf_r_azim_stdev (float, default: 20.0)

• Azimuth angle measurement noise standard deviation, in degrees.

~kf_r_elev_stdev (float, default: 20.0)

• Elevation angle measurement noise standard deviation, in degrees.